import cv2, numpy
from cv_bridge import CvBridge

import rclpy
from rclpy.node import Node

from carsource_interfaces.msg import CameraOriInterface



class SubImageNode(Node):
    def __init__(self):
        super().__init__("subImagelanDetec")

        
        self.br = CvBridge()
        self.camera_subscriber = self.create_subscription(CameraOriInterface, 'camera_ori_data', self.image_callback, 10)         



          
    def image_callback(self, msg:CameraOriInterface):

        image = self.br.imgmsg_to_cv2(msg.image)
        hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
        # 巡检的阈值 
        lower_black = numpy.array([0, 0, 0])
        upper_black = numpy.array([180, 255, 46])

        mask = cv2.inRange(hsv, lower_black, upper_black)

        h, w, d = image.shape
        search_top = 3*h/4                                                                                                  
        search_bot = search_top + 20
        print(str(search_top) + " : " + str(search_bot))
        mask[0:int(search_top), 0:w] = 0
        mask[int(search_bot):h, 0:w] = 0

        M = cv2.moments(mask)
        if M['m00'] > 0:
            cx = int(M['m10']/M['m00'])
            cy = int(M['m01']/M['m00'])
            cv2.circle(mask, (cx, cy), 20, (0,0,255), -1)
            vtherror = cx - w/2
            print(cx,cy)
            # self.twist.linear.x = self.forward_velocity
            # self.twist.angular.z = -float(vtherror) / 300 * self.scale_diversion   # 400: 0.1, 300: 0.15, 250, 0.2
            # self.cmd_vel_pub.publish(self.twist)
        else:
            print('not found line')            
            # self.twist.linear.x = 0.0
            # self.twist.angular.z = 0.0
            # self.cmd_vel_pub.publish(self.twist)
        
        cv2.imshow("mask", mask)
        cv2.waitKey(1)
    

    
    

def main():
    rclpy.init()
    rclpy.spin(SubImageNode())
    rclpy.shutdown()


if __name__ == '__main__':
    main()
